#include <DeformingPolygon.h>
#include <map>
#include <set>
#include <szMiscOperations.h>
#include <szMexUtility.h>
#include <IntersectionConvexPolygons.h>
#include <DisjointSet.h>
#include <mex.h> //just for printf


/*
A unit vector from o to x in 2D
*/
CParticleF 
NormalizedDirection(CParticleF& x, CParticleF& o)
{
	float dx = x.m_X - o.m_X;
	float dy = x.m_Y - o.m_Y;
	float len = sqrt(dx*dx + dy*dy);
	if(len > 1.0e-5)
	{
		return CParticleF(dx/len, dy/len);
	}
	else
	{
		return CParticleF(0.0f, 0.0f);
	}
}

/*
A unit vector from o to x in 3D
*/
CParticleF 
NormalizedDirection3D(CParticleF& x, CParticleF& o)
{
	float dx = x.m_X - o.m_X;
	float dy = x.m_Y - o.m_Y;
	float dz = x.m_Z - o.m_Z;
	float len = sqrt(dx*dx + dy*dy + dz*dz);
	if(len > 1.0e-5)
	{
		return CParticleF(dx/len, dy/len, dz/len);
	}
	else
	{
		return CParticleF(0.0f, 0.0f, 0.0f);
	}
}

/*
return a unit vector that bisects the angle formed by three points: a-o-b.
*/
CParticleF
bisector(CParticleF& o, CParticleF& a, CParticleF& b)
{
	CParticleF x = NormalizedDirection(a, o);
	CParticleF y = NormalizedDirection(b, o);
	CParticleF z((x.m_X+y.m_X)/2, (x.m_Y+y.m_Y)/2);
	float vx = z.m_X;
	float vy = z.m_Y;
	float len0 = sqrt(vx*vx + vy*vy);
	if(len0 <= 1.0e-5) //this is a colinear point. 
	{
		float ang = GetVisualDirection(b.m_X, b.m_Y, a.m_X, a.m_Y) + PI/2.0;
		vx = cos(ang);
		vy = sin(ang);
	}
	else
	{
		vx = vx / len0;
		vy = vy / len0;
	}
	return CParticleF(vx, vy);
}

float
intersectPlaneAndLine(CParticleF op, CParticleF& z, CParticleF& ol, CParticleF u)
{
	//move the origin to ol.
	op.m_X -= ol.m_X;
	op.m_Y -= ol.m_Y;
	op.m_Z -= ol.m_Z;
	float t = (z.m_X * op.m_X + z.m_Y * op.m_Y + z.m_Z * op.m_Z) / (z.m_X * u.m_X + z.m_Y * u.m_Y + z.m_Z * u.m_Z);
	return t;
}

/*
return true if x is on a line segment between [p, q).
*/
bool
overlap(CParticleF& x, CParticleF& p, CParticleF& q, float eps)
{
	float a = p.m_X;
	float b = q.m_X-p.m_X;
	float c = p.m_Y;
	float d = q.m_Y-p.m_Y;
	float det = b*b + d*d;
	if(det < 1.0e-5) //p and q are too close
	{
		return Distance(x, p) < eps;
	}
	float t = (-a*b - c*d + b*x.m_X + d*x.m_Y)/(b*b + d*d);
	CParticleF y(a+b*t, c+d*t);
	if(t >=0 && t<1.0)
	{
		if(Distance(x, y) < eps) return true;
		else return false;
	}
	else return false;
}

bool
coLinear(CParticleF& a, CParticleF& b, CParticleF& c, float precision = 1.0e-3)
{
	float d = Distance2Line(a, c, b);
	return d < precision;
}

bool
coLinear(CParticleF& a, CParticleF& b, CParticleF& c, CParticleF& d, float precision = 1.0e-3)
{
	float d1 = Distance2Line(a, c, b);
	float d2 = Distance2Line(a, c, d);
	return d1 < precision && d2 < precision;
}

CParticleF
crossProduct(CParticleF& a, CParticleF& b)
{
	return CParticleF(a.m_Y*b.m_Z-a.m_Z*b.m_Y, a.m_Z*b.m_X-a.m_X*b.m_Z, a.m_X*b.m_Y-a.m_Y*b.m_X);
}

/*
Given three points in 3D, define a unit vector perpendicular to the plane going through the three points.
The vector is always upward (i.e. its Z-coordinate is always positive). But this should not really matter...
*/
CParticleF
perp2Plane(CParticleF& a, CParticleF& b, CParticleF& c)
{
	CParticleF u = NormalizedDirection3D(b, a);
	CParticleF v = NormalizedDirection3D(c, a);
	CParticleF z = crossProduct(u, v);
	if(z.m_Z < 0)
	{
		z.m_X = -z.m_X;
		z.m_Y = -z.m_Y;
		z.m_Z = -z.m_Z;
	}
	return z;
}

bool
calculateVelocity(CParticleF& a, CParticleF& o, CParticleF& b, float& vx, float& vy)
{
	CParticleF bs = bisector(o, a, b);
	float ang = GetVisualAngle2(a.m_X, a.m_Y, b.m_X, b.m_Y, o.m_X, o.m_Y);
	float cs = cos((PI - Abs(ang)) / 2.0);
	float len = 1.0f / cs;
	bs.m_X *= len;
	bs.m_Y *= len;
	if(ang<0)
	{
		bs.m_X = -bs.m_X;
		bs.m_Y = -bs.m_Y;
	}
	vx = bs.m_X;
	vy = bs.m_Y;
	return true;
}

bool 
MovingParticle::setVelocity()
{
	if(next && prev)
	{
		if(next == prev)
		{
			NormalizedDirection(next->p, p);
		}
		else
		{
			calculateVelocity(prev->p, p, next->p, v[0], v[1]);
		}
		return true;
	}
	else
	{
		v[0] = 0; v[1] = 0;
		return false;
	}
}


DeformingPolygon::DeformingPolygon(vector<CParticleF>& pnts)
{
	ParticleFactory* factory = ParticleFactory::getInstance();
	float eps = 1.0e-5;
	for(int i=0; i<pnts.size(); ++i)
	{
		MovingParticle* p = factory->makeParticle(pnts[i]);
		particles.push_back(p);
	}
	for(int i=0; i<particles.size(); ++i)
	{
		//particles[i]->p0 = particles[i]->p;
		int j = (i+1) % particles.size();
		particles[i]->next = particles[j];
		int k = (i-1+particles.size()) % particles.size();
		particles[i]->prev = particles[k];
		particles[i]->setVelocity();
	}
}

pair<float,MovingParticle*>
DeformingPolygon::nextEdgeEvent()
{
	float t = std::numeric_limits<float>::infinity();
	pair<float,MovingParticle*> res(t, (MovingParticle*)NULL);
	for(int i=0; i<particles.size(); ++i)
	{
		if(particles[i]->id+1==402)
			i+=0;
		CParticleF p2 = particles[i]->move(1);
		CParticleF q2 = particles[i]->next->move(1);
		pair<float,float> param = _IntersectConvexPolygon::intersect(particles[i]->p, p2, particles[i]->next->p, q2);
		float t0 = Max(param.first, param.second);
		if(param.first>0 && param.second >0 && t0 < t)
		{
			t = t0;
			res.first = t;
			res.second = particles[i];
		}
	}
	return res;
}

pair<float,MovingParticle*>
DeformingPolygon::nextSplitEvent()
{
	float t = std::numeric_limits<float>::infinity();
	pair<float,MovingParticle*> res(t, (MovingParticle*)NULL);
	for(int i=0; i<particles.size(); ++i)
	{
		CParticleF bs = bisector(particles[i]->p, particles[i]->prev->p, particles[i]->next->p);
		if(bs.m_X * particles[i]->v[0] + bs.m_Y * particles[i]->v[1] < 0) //concave
		{
			CParticleF ol = particles[i]->p;
			CParticleF ul(particles[i]->v[0], particles[i]->v[1], 1.0);
			for(int j=0; j<particles.size(); ++j)
			{
				if(particles[i]==particles[j] || particles[i]->next==particles[j] || particles[i]->prev==particles[j]) continue;
				CParticleF op = particles[j]->p;
				CParticleF oq = particles[j]->next->p;
				CParticleF or(op.m_X + particles[j]->v[0], op.m_Y + particles[j]->v[1], 1.0f);
				CParticleF up = perp2Plane(op, oq, or);
				float t0 = intersectPlaneAndLine(op, up, ol, ul);
				if(t0 > 0 && t0<t) 
				{
					//check if the intersection point is within the sweep of the line segment.
					CParticleF loc(ol.m_X+t0*ul.m_X, ol.m_Y+t0*ul.m_Y);//the location of intersection in 2D
					CParticleF v1(particles[j]->v[0], particles[j]->v[1], 0.0f); 
					CParticleF dp1 = NormalizedDirection(loc, op);
					CParticleF cp1 = crossProduct(v1, dp1);
					CParticleF v2(particles[j]->next->v[0], particles[j]->next->v[1], 0.0f); 
					CParticleF dp2 = NormalizedDirection(loc, oq);
					CParticleF cp2 = crossProduct(v2, dp2);
					if(cp1.m_Z * cp2.m_Z < 0 || Abs(cp1.m_Z) < 1.0e-5 || Abs(cp2.m_Z) < 1.0e-5)
					{
						t = t0;
						res.first = t;
						res.second = particles[i];
					}
				}
			}
		}
	}
	return res;
}

bool
DeformingPolygon::sanityCheck(int& error_code, int& error_loc)
{
	error_code = 0;
	error_loc = 0;
	map<MovingParticle*,bool> visited;
	{
		for(int i=0; i<particles.size(); ++i)
		{
			visited[particles[i]] = false;
		}
		int count = 0;
		MovingParticle* start = particles[0];
		MovingParticle* p = start;
		do
		{
			visited[p] = true;
			p = p->next;
			count++;
		}
		while(count < particles.size() && p != start);
		if(count < particles.size()) //premature ending
		{
			error_code = 1;
			return false;
		}
		else if(p != start) //unfinished
		{
			error_code = 2;
			return false;
		}
	}
	{
		for(int i=0; i<particles.size(); ++i)
		{
			visited[particles[i]] = false;
		}
		int count = 0;
		MovingParticle* start = particles[0];
		MovingParticle* p = start;
		do
		{
			visited[p] = true;
			p = p->prev;
			count++;
		}
		while(count < particles.size() && p != start);
		if(count < particles.size()) //premature ending
		{
			error_code = 3;
			return false;
		}
		else if(p != start) //unfinished
		{
			error_code = 4;
			return false;
		}
	}
	return true;
}

void
quickFinish(vector<MovingParticle*> particles)
{
	ParticleFactory* factory = ParticleFactory::getInstance();
	if(particles.size()==2)
	{
		MovingParticle* p1 = factory->makeParticle(particles[0]->p);
		p1->p = particles[1]->p;
		CParticleF v = NormalizedDirection(p1->p, p1->p0);
		p1->v[0] = v.m_X;
		p1->v[1] = v.m_Y;
	}
	else if(particles.size()==3)
	{
		CParticleF q0 = particles[1]->move(1.0f);
		CParticleF q1 = particles[2]->move(1.0f);
		pair<float,float> param = _IntersectConvexPolygon::intersect(particles[0]->p, q0, particles[1]->p, q1);
		particles[1]->p = particles[1]->move(param.first);
		particles[2]->p = particles[1]->p;
		particles[0]->p = particles[1]->p;
	}
}

//a utility struct to keep some useful info
struct _overlapInfo
{
	_overlapInfo(MovingParticle* p)
	{
		this->p = p;
		front = NULL;
		back = NULL;
		_overlap = false;
		_entry = false;
		_exit = false;
		_disabled = false;
	}
	MovingParticle* p;
	MovingParticle* contact; //the first point of the contacting segment
	MovingParticle* front; //next particle after re-routing
	MovingParticle* back; //prev particle after re-routing
	bool _overlap;
	bool _entry;
	bool _exit;
	bool _disabled;
};

vector<MovingParticle*>
vectorize(MovingParticle* start)
{
	MovingParticle* p = start;
	vector<MovingParticle*> tr;
	set<MovingParticle*> pset;
	bool success = true;
	do
	{
		if(pset.find(p) != pset.end()) //premature loop is found
		{
			success = false;
			break;
		}
		if(p->next == NULL)
		{
			success = false;
			break;
		}
		tr.push_back(p);
		pset.insert(p);
		p = p->next;
	}
	while(p != start);

	if(success == false)
	{
		mexErrMsgTxt("vectorize: failed to vectorize a shape.");
	}
	return tr;
}

vector<MovingParticle*>
removeDuplicates(vector<MovingParticle*>& particles, float eps)
{
	//first make sure they are not all the same. if so, return a vector with the first guy.
	bool bdif = false;
	for(int i=0; i<particles.size()-1; ++i)
	{
		if(Distance(particles[i]->p, particles[i+1]->p) > eps)
		{
			bdif = true;
			break;
		}
	}
	if(bdif == false)
	{
		vector<MovingParticle*> v(1, particles[0]);
		return v;
	}

	//now we know there are at least two distinct particles so that the loops never go infinite.
	ParticleFactory* factory = ParticleFactory::getInstance();
	MovingParticle* start = particles[0];
	MovingParticle* p = start;
	int count = 0;
	while(count < particles.size())
	{
		count++;
		MovingParticle* prev = p->prev;
		while(Distance(p->p, prev->p) < eps)
		{
			prev = prev->prev;
			count++;
		}
		MovingParticle* next = p->next;
		while(Distance(p->p, next->p) < eps)
		{
			next = next->next;
			count++;
		}
		if(p->prev != prev || p->next != next) //duplicate
		{
			MovingParticle* q = factory->makeParticle(p->p);
			q->next = next;
			q->prev = prev;
			q->next->prev = q;
			q->prev->next = q;
			q->setVelocity();
			if(p == start) start = q; //new start particle
			p = q->next;
		}
		else
		{
			p = p->next;
		}
	}
	return vectorize(start);
}

/*
Loop through the current polygon, find any points that have overlaps (either splitting by a concave edge or merging by parallel edges),
and collect entry and exiting points of each overlap. 
For each entry and exiting point, provide a bridging particle to be inserted.
After the bridges are inserted, trace the particles once more and divide it into multiple polygons.
*/
vector<DeformingPolygon>
fixPolygon(DeformingPolygon& polygon, float eps)
{
	ParticleFactory* factory = ParticleFactory::getInstance();
	vector<_overlapInfo> vinfo;
	map<MovingParticle*,int> imap;
	int N = polygon.particles.size();
	for(int i=0; i<N; ++i)
	{
		imap[polygon.particles[i]] = i;
		vinfo.push_back(_overlapInfo(polygon.particles[i]));
	}
	//float eps = 1.0e-3;
	for(int i=0; i<N; ++i)
	{
		MovingParticle* p = polygon.particles[i];
		for(int j=0; j<N; ++j)
		{
			MovingParticle* q = polygon.particles[j];
			if(p==q || q->next==p) continue;

			if(overlap(p->p, q->p, q->next->p, eps))
			{
				vinfo[i].contact = q;
				vinfo[i]._overlap = true;
				break;
			}
		}
	}
	for(int i=0; i<N; ++i)
	{
		int i0 = (i-1+N) % N;
		int i2 = (i+1) % N;
		if(vinfo[i]._overlap)
		{
			if(vinfo[i0]._overlap==false)
			{
				vinfo[i]._entry = true;
				vinfo[i].back = vinfo[i].p->prev;
				vinfo[i].front = vinfo[i].contact->next;
			}
			if(vinfo[i2]._overlap==false)
			{
				vinfo[i]._exit = true;
				vinfo[i].back = vinfo[i].contact;
				vinfo[i].front = vinfo[i].p->next;
			}
		}
	}
	//remove redundant ones
	vector<_overlapInfo> seeds;
	for(int i=0; i<N; ++i)
	{
		if((vinfo[i]._entry || vinfo[i]._exit) && vinfo[i]._disabled==false)
		{
			seeds.push_back(vinfo[i]);
			int j = imap[vinfo[i].contact];
			if(vinfo[j].contact == vinfo[i].p) //redundant
			{
				if(coLinear(vinfo[i].p->p, vinfo[i].front->p, vinfo[i].back->p))
				{
					vinfo[i]._disabled = true;
				}
				else
				{
					vinfo[j]._disabled = true;
				}
			}
		}
	}

	vector<vector<MovingParticle*>> polygons;
	//vector<bool> visited(N, false);
	vector<MovingParticle*> longest;
	if(seeds.size()==3)
		seeds.erase(seeds.end()-1); //TK: DEBUGGING...
	for(int i=0; i<seeds.size(); ++i)
	{
		MovingParticle* start = factory->makeParticle(seeds[i].p->p);
		start->next = seeds[i].front;
		start->prev = seeds[i].back;
		start->next->prev = start;
		start->prev->next = start;
		vector<MovingParticle*> tr = vectorize(start);

		tr = removeDuplicates(tr, eps);
		start->setVelocity();		
		if(tr.size() > longest.size())
		{
			longest = tr;
		}
		if(tr.size() <= 3)
		{
			quickFinish(tr);
		}
		else
		{
			//update the velocity
			polygons.push_back(tr);
		}
	}

	vector<DeformingPolygon> shapes;
	if(polygons.size() > 0)
	{
		polygon.particles = longest;
		for(int i=0; i<polygons.size(); ++i)
		{
			if(polygons[i][0]!=longest[0])
			{
				DeformingPolygon dp;
				dp.particles = polygons[i];
				shapes.push_back(dp);
			}
		}
	}
	return shapes;
}

void
_trace(Vertex<CParticleF>* u, Vertex<CParticleF>*prev, vector<CParticleF>& points)
{
	points.push_back(u->key);
	u->color = Black;
	vector<Edge<CParticleF>*> edges;
	if(prev == NULL)
	{
		edges = u->aList;
	}
	else //sort edges in clock wise order
	{
		vector<pair<float,Edge<CParticleF>*>> pairs;
		for(int i=0; i<u->aList.size(); ++i)
		{
			Vertex<CParticleF>* v = u->aList[i]->v;
			float ang = GetVisualAngle2(prev->key.m_X, prev->key.m_Y, v->key.m_X, v->key.m_Y, u->key.m_X, u->key.m_Y);
			if(ang <= 0) ang += 2*PI;
			pairs.push_back(pair<float,Edge<CParticleF>*>(ang, u->aList[i]));
		}
		sort(pairs.begin(), pairs.end());
		for(int i=0; i<pairs.size(); ++i)
		{
			u->aList[i] = pairs[i].second;
		}
	}

	for(int i=0; i<u->aList.size(); ++i)
	{
		Vertex<CParticleF>* v = u->aList[i]->v;
		if(v->color == White)
		{
			_trace(v, u, points);
			points.push_back(u->key);
		}
	}
}

vector<CParticleF>
traceTree(vector<Vertex<CParticleF>*>& tree)
{
	for(int i=0; i<tree.size(); ++i)
	{
		tree[i]->Reset();
	}

	vector<CParticleF> points;
	_trace(tree[0], NULL, points);
	//remove a copy of the first one at the end of the trace
	points.erase(points.end()-1);
	return points;
}

vector<CParticleF>
offsetPolygon(vector<CParticleF>& points, float margin, float eps)
{
	vector<CParticleF> poly;
	for(int i=0; i<points.size(); ++i)
	{
		int i0 = (i-1+points.size()) % points.size();
		int i2 = (i+1) % points.size();
		CParticleF o = points[i];
		CParticleF a = points[i0];
		CParticleF b = points[i2];
		if(Distance(a, o) < eps || Distance(b, o) < eps) 
		{
			continue;
		}
		if(Distance(a, b) > eps)
		{
			float vx, vy;
			calculateVelocity(a, o, b, vx, vy);
			CParticleF q(o.m_X + margin * vx, o.m_Y + margin * vy);
			poly.push_back(q);
			//printf("o[%3.3f,%3.3f] -> [%3.3f,%3.3f]\n", o.m_X, o.m_Y, q.m_X, q.m_Y);
		}
		else //this is a leaf node of the tree.
		{
			float ang = GetVisualDirection(o.m_X, o.m_Y, a.m_X,  a.m_Y);
			float angles[] = {ang+PI/4.0, ang-PI/4.0};
			for(int j=0; j<2; ++j)
			{
				CParticleF q(o.m_X+sqrt(2.0) * margin*cos(angles[j]), o.m_Y+sqrt(2.0)*margin*sin(angles[j]));
				//printf("\to[%3.3f,%3.3f] -> [%3.3f,%3.3f]\n", o.m_X, o.m_Y, q.m_X, q.m_Y);
				poly.push_back(q);
			}
		}
	}
	return poly;
}


